


#ifndef _FILTER_H_
#define _FILTER_H_









#include "sl_matrix.h"

/// <summary>
/// C语言接口的卡尔曼滤波器
/// </summary>
typedef struct _kalman_filter_vec_
{

    /*
    // 两个估计的高斯分布的矢量x1和x2，协方差矩阵分别是s1和s2。
    //则最优估计的协方差是  s12 = {s1^(-1)+s2^(-1)}^(-1) = s1 * {s1+s2}^(-1) * s2  , 最优估计矢量是 s12*（s1^(-1)*x2 + s2^(-1)*x1）

    //卡尔曼滤波： 定义增益矩阵K = s1 * {s1+s2}^(-1).  ==>>  I-K = {s1+s2}*{s1+s2}^(-1) -s1*{s1+s2}^(-1) = s2 * {s1+s2}^(-1)
    // s12 = (I-K)*s1
    // x12 = (I-K)*x1 + K*x2 = x1+K*(x2-x1)

    // 卡尔曼滤波的模型要复杂一些：

    设系统是线性的，k时刻的状态x[k] 和k-1时刻的状态x[k-1]可以用下面的公式相联系：
    x[k] = F*x[k-1]+B*u[k-1]+w[k-1]  >>   x[k]是k时刻的状态，x[k-1]是k-1时刻的状态，F是状态转移矩阵，B是控制矩阵，u是控制量，w是过程噪声。
    z[k] = H*x[k]+v[k]               >>   z[k]是观测，H是观测矩阵，v是观测噪声。
    过程噪声 w[k] 的协方差矩阵是Q，观测噪声 v[k] 的协方差是R。

    卡尔曼滤波推导：根据k时刻观测z[k],上一个最优估计x[k-1]，过程噪声 w[k]，观测噪声 v[k]，预测最新的状态 x[k]。
    预测模型：
    x_[k] = F*x[k-1] + B*u[k-1] + w[k]    >>  x[k-1]是[k-1]时刻的最优估计。x_[k]是k时刻的预计，要结合观测z[k]来做最优估计。w[k]是均值为0的噪声。
    P_[k] = F*P[k-1]F^T + Q               >>  Q是过程噪声w协方差，P_[k]是预测的协方差，P[k-1]是[k-1]时刻的协方差。

    参数更新：
    k时刻卡尔曼增益：      K[k] = P_[k] * H^T * ( H*P_[k]*H^T + R )^(-1)  >> 这里R是观测噪声的协方差。
    k时刻预测估计：        x[k] = x_[k] + K[k]* (z[k]-H*x_[k])             >> x_[k] 是预计，x[k]是最优估计。
    k时刻最优估计的协方差：P[k] = (I - K[k]*H)* P_[k]                      >> P_[k] 预计的协方差。P[k]最优估计的协方差。

    */

    int state_d;    //状态的维度n
    int input_d;    //控制量的维度c
    int observation_d;    //观测状态的维度m

    mat last_p;    //上次最优估计协方差，n*n
    mat now_p;     //当前最优估计协方差，n*n

    vec input;         //控制量的输入，c维
    vec observe;       //观测的输入，m维

    vec now_state;       //当前状态最优估计，n维
    vec last_state;      //上一个状态最优估计，n维

    mat K;        //卡尔曼增益，n*m
    mat Q;        //过程噪声协方差，n*n，这个是状态转移过程的噪声。
    mat R;        //观测噪声协方差，m*m，这个是观测的噪声。    

    mat B;        //控制矩阵，n*c
    mat H;        //观测矩阵，m*n
    mat F;        //状态转移矩阵，n*n


    // 下面是滤波计算的缓存变量。

        //预测：
    vec estimate_s;            //预测状态  .  estimate 预测
    mat estimate_p;   //预测协方差。
    mat nn;
    mat nnt;
    //更新参数：
    mat pht;
    mat ht;
    mat hti;
    mat hpht;
    mat mm;
    mat imm;
    mat kh;
    mat I;

    vec estimate_z;
    vec z_estimate_z;
}kalman_vec;

/// <summary>
/// 初始化卡尔曼滤波器，并分配内存。
/// </summary>
/// <param name="kalman">卡尔曼滤波器</param>
/// <param name="d">状态样本的维度</param>
/// <param name="id">控制数据的维度</param>
/// <param name="od">观测数据的维度</param>
void kalman_vec_init(kalman_vec* kalman, int d, int id, int od);

/// <summary>
/// 释放卡尔曼滤波器的内存。
/// </summary>
/// <param name="kalman">卡尔曼滤波器</param>
void kalman_vec_release(kalman_vec* kalman);


/// <summary>
/// 根据观测数据初始化方差。用obs[]的协方差矩阵当作观测噪声和过程噪声，及初始状态的噪声。
/// </summary>
/// <param name="kalman">卡尔曼结构体</param>
/// <param name="obs">观测数据序列</param>
/// <param name="obs_len">观测数据序列长度 </param>
void kalman_vec_init_para(kalman_vec* kalman, vec obs[], int obs_len);

/// <summary>
/// 带控制输入的卡尔曼滤波器
/// </summary>
/// <param name="kalman">滤波器</param>
/// <param name="obs">观测矢量</param>
/// <param name="inp">控制矢量</param>
void kalman_vec_filter_with_input(kalman_vec kalman, vec obs, vec inp);

/// <summary>
/// 无输入的卡尔曼滤波器，控制矢量初始化为0： vec_init(kalman.input, 0);
/// </summary>
/// <param name="kalman">滤波器</param>
/// <param name="obs">观测矢量</param>
void kalman_vec_filter(kalman_vec kalman, vec obs);


void test_kalman_vec(void);


typedef struct _kalman_filter_float_
{
    float last_p;    //上次最优估计协方差，n*n
    float now_p;     //当前最优估计协方差，n*n

    float now_state;       //当前状态最优估计，n维
    float last_state;      //上一个状态最优估计，n维

    float input;         //控制量的输入，c维
    float observe;       //观测的输入，m维

    float K;        //卡尔曼增益，n*m
    float Q;        //过程噪声协方差，n*n ，这个是状态转移过程的噪声。
    float R;        //观测噪声协方差，m*m

    float B;          //控制矩阵，n*c
    float H;          //观测矩阵，m*n
    float F;          //状态转移矩阵，n*n

}kalman_float;



/// <summary>
/// 初始化卡尔曼滤波
/// </summary>
/// <param name="q">过程噪声方差</param>
/// <param name="r">观测噪声方差</param>
/// <param name="nowp">初始状态方差，可以设大一些</param>
/// <param name="nows">当前状态</param>
void kalman_float_init(kalman_float* kalman, float q, float r, float nowp, float nows);


/// <summary>
/// 卡尔曼滤波
/// </summary>
/// <param name="kalman">滤波器</param>
/// <param name="obs">观测量</param>
/// <param name="inp">控制量</param>
void kalman_float_filter_with_input(kalman_float* kalman, float obs, float inp);


/// <summary>
/// 卡尔曼滤波
/// </summary>
/// <param name="kalman">滤波器</param>
/// <param name="obs">观测量</param>
void kalman_float_filter(kalman_float* kalman, float obs);








#endif	


